clc;
clear;
% 2021.10.10
% 本程序是HW1作业

% 参数：1200kg、(200, 200, 150)kN/m、0.02, 0.02, 0.04
m = 1200;
mass_m = m * eye(3);
k1 = 200 * 1000;
k2 = 200 * 1000;
k3 = 150 * 1000;
stif_m = [k1+k2 -k2 0;
    -k1 k2+k3 -k3;
    0 -k3 k3];
% stif_m = 1000*[400 -200 0;
%     -200 350 -150;
%     0 -150 150];
% mass_m\stif_m
% 阻尼比
cri_damp_m = [0.02, 0.02, 0.04];
% 自振频率
% inv(mass_m)*mass_m=eye(3)
% [eig_vec, eig_val] = eig(inv(mass_m)*stif_m);
[eig_vec, eig_val] = eig(mass_m\stif_m);

% 如何对应频率
omega_m = sort(sqrt(diag(eig_val)))';     % 5.6419   14.7706   22.3607

% %%
% i=1;
% j=3;
% % 瑞丽阻尼
% alpha = 2*omega_m(i)*omega_m(j)/(omega_m(j)^2-omega_m(i)^2)*(omega_m(j)*cri_damp_m(i)-omega_m(i)*cri_damp_m(j));
% beta = 2*omega_m(i)*omega_m(j)/(omega_m(j)^2-omega_m(i)^2)*(-cri_damp_m(i)/omega_m(j)+cri_damp_m(j)/omega_m(i));
% damp_m = alpha * mass_m + beta * stif_m;
% 
% % state matrix
% A  = [zeros(3) eye(3);
%     -stif_m/mass_m -damp_m/mass_m];
% % input matrix
% B = [zeros(1,3) 1 1 1]'/m;
% % 课上demo没有除以m？
% % B = [zeros(1,3) 1 1 1]';
% % output matrix [x1' x2' x3' x1" x2" x3"]
% C = [0 0 0 0 1 0];
% % throughtput matrix ?
% D = 0;
% % ss_structural
% state_ss = ss(A, B, C, D);
% trans_ss = tf(state_ss);
% 
% % conmput
% t = 1:0.01:10;
% P = 10*sin(2*pi*t);
% % X0 = [0, 0, 0, 0, 0, 10];
% X0 = [0, 0, 0, 0, 0, 0];
% % P = 10 * heaviside(t-2);
% % lsim X0?
% out_S = lsim(state_ss, P, t, X0);
% out_T = lsim(trans_ss, P, t, X0);
% 
% % state space 与 tranform function 计算结果一致
% % lsim设置初始位置时不同
% subplot(2, 2, 1);
% plot(t, P, t, out_S)
% subplot(2, 2, 2);
% plot(t, P, t, out_T)
% subplot(2, 2, 3);
% bode(state_ss)
% subplot(2, 2, 4);
% bode(trans_ss)

%%
% problem_b
% 相同大小激励下，分别对1、3施加，在另一端观测

i=1;
j=2;
% 瑞丽阻尼
alpha = 2*omega_m(i)*omega_m(j)/(omega_m(j)^2-omega_m(i)^2)*(omega_m(j)*cri_damp_m(i)-omega_m(i)*cri_damp_m(j));
beta = 2*omega_m(i)*omega_m(j)/(omega_m(j)^2-omega_m(i)^2)*(-cri_damp_m(i)/omega_m(j)+cri_damp_m(j)/omega_m(i));
damp_m = alpha * mass_m + beta * stif_m    % 19.5959   19.5959   33.9411

A  = [zeros(3) eye(3);
    -stif_m/mass_m -damp_m/mass_m];
% 施加在3
B3 = [zeros(1,3) 0 0 1]'/m;
C3 = [1 0 0 0 0 0];
% 施加在1
B1 = [zeros(1,3) 1 0 0]'/m;
C1 = [0 0 1 0 0 0];
D = 0;

state_f1 = ss(A, B1, C1, D);
state_f3 = ss(A, B3, C3, D);
tf(state_f1)
tf(state_f3)

% t = 1:0.01:10;
% P = 100*sin(100*t);
% % H_x3_f1(w)
% out_S1 = lsim(state_f1, P, t);
% % H_x1_f3(w)
% out_S3 = lsim(state_f3, P, t);

% same Bode Diagram 波特图物理意义？
subplot(1, 2, 1);
bode(state_f1)
subplot(1, 2, 2);
bode(state_f3)

%%
% problem_c
% 
% H_x1_f3(w)
% 使用freqresp，并绘制传递函数实部-频率图
state_f3 = ss(A, B3, C3, D);
tf_f3 = tf(state_f3);
w_speci = 0:0.0001:130;
H = freqresp(tf_f3, w_speci);

% class(H)  % 'double'
% size(H)   % 1     1   118
real_H = reshape(real(H), size(w_speci));
imag_H = reshape(imag(H), size(w_speci));

subplot(2, 2, 1)
plot(w_speci, real_H)
title('w-real_H')
xlabel('w')
ylabel('real_H')
% w_out(118)=1000?
% xlim([0 max(wout)])
ylim([-max(real_H) max(real_H)])
subplot(2, 2, 2)
plot(w_speci, imag_H)
title('w-imag_H')
xlabel('w')
ylabel('imag_H')
subplot(2, 2, 3)
plot3(real_H, imag_H, w_speci)
title('real_H-imag_H-w')
grid on
xlabel('real_H')
ylabel('imag_H')
zlabel('w')
subplot(2, 2, 4)
plot(real_H, imag_H)
title('real_H-imag_H')
xlabel('real_H')
ylabel('imag_H')

% figure(2)
% nyquist(tf_f3)
% xlim([-0.0005,0.0005])

%%
% problem_d
i=2;
j=3;
% 瑞丽阻尼?
alpha = 2*omega_m(i)*omega_m(j)/(omega_m(j)^2-omega_m(i)^2)*(omega_m(j)*cri_damp_m(i)-omega_m(i)*cri_damp_m(j));
beta = 2*omega_m(i)*omega_m(j)/(omega_m(j)^2-omega_m(i)^2)*(-cri_damp_m(i)/omega_m(j)+cri_damp_m(j)/omega_m(i));
damp_m = alpha * mass_m + beta * stif_m;    % 19.5959   19.5959   33.9411

A  = [zeros(3) eye(3);
    -stif_m/mass_m -damp_m/mass_m];

% 施加在1
B1 = [zeros(1,3) 1 0 0]'/m;
C1 = [0 0 0 1 0 0];
% 施加在2
B2 = [zeros(1,3) 0 1 0]'/m;
C2 = [0 0 0 0 1 0];
% 施加在3
B3 = [zeros(1,3) 0 0 1]'/m;
C3 = [0 0 0 0 0 1];
D = 0;

state_f1_1 = ss(A, B1, C1, D);
state_f2_2 = ss(A, B2, C2, D);
state_f3_3 = ss(A, B3, C3, D);

% same Bode Diagram 波特图物理意义？
subplot(1, 3, 1);
bode(state_f1_1)
subplot(1, 3, 2);
bode(state_f2_2)
subplot(1, 3, 3);
bode(state_f3_3)
